@ Standard definitions of Mode bits and Interrupt (I & F) flags in PSRs

.equ Mode_USR,            0x10
.equ Mode_FIQ,            0x11
.equ Mode_IRQ,            0x12
.equ Mode_SVC,            0x13
.equ Mode_ABT,            0x17
.equ Mode_UND,            0x1B
.equ Mode_SYS,            0x1F

.equ I_Bit,               0x80            @ when I bit is set, IRQ is disabled
.equ F_Bit,               0x40            @ when F bit is set, FIQ is disabled


@// <h> Stack Configuration (Stack Sizes in Bytes)
@//   <o0> Undefined Mode      <0x0-0xFFFFFFFF:8>
@//   <o1> Supervisor Mode     <0x0-0xFFFFFFFF:8>
@//   <o2> Abort Mode          <0x0-0xFFFFFFFF:8>
@//   <o3> Fast Interrupt Mode <0x0-0xFFFFFFFF:8>
@//   <o4> Interrupt Mode      <0x0-0xFFFFFFFF:8>
@//   <o5> User/System Mode    <0x0-0xFFFFFFFF:8>
@// </h>

.equ UND_Stack_Size,     0x00000200
.equ SVC_Stack_Size,     0x00000200
.equ ABT_Stack_Size,     0x00000000
.equ FIQ_Stack_Size,     0x00000200
.equ IRQ_Stack_Size,     0x00000200
.equ USR_Stack_Size,     0x00000200

.equ Stack_Size,        (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + \
                         FIQ_Stack_Size + IRQ_Stack_Size + USR_Stack_Size)

@@                AREA    STACK, NOINIT, READWRITE, ALIGN=3
@@
@@Stack_Mem       SPACE   Stack_Size
@@Stack_Top       EQU     Stack_Mem + Stack_Size

            .arm
            .section .STACK, "w"
.align 3
Stack_Mem:  .space Stack_Size
.equ Stack_Top, Stack_Mem + Stack_Size


@// <h> Heap Configuration
@//   <o>  Heap Size (in Bytes) <0x0-0xFFFFFFFF>
@// </h>

@@Heap_Size       EQU     0x00000000
@@                AREA    HEAP, NOINIT, READWRITE, ALIGN=3
@@Heap_Mem        SPACE   Heap_Size

.equ Heap_Size,   0x00000000
            .section .HEAP, "w"
            .align 3
HeapMem:
    .if (Heap_Size>0)
        .space Heap_Size
    .endif


@ Area Definition and Entry Point
@  Startup Code must be linked first at Address at which it expects to run.

@@                AREA    RESET, CODE, READONLY
@@                ARM
            .section .RESET, "ax"
            .arm

@ Exception Vectors
@  Mapped to Address 0.
@  Absolute addressing mode must be used.
@  Dummy Handlers are implemented as infinite loops which can be modified

        .global _boot
_boot:
boot:
Vectors:        LDR     PC, Reset_Addr         
                LDR     PC, Undef_Addr
                LDR     PC, SWI_Addr
                LDR     PC, PAbt_Addr
                LDR     PC, DAbt_Addr
                NOP                            @ Reserved Vector 
                LDR     PC, IRQ_Addr
@@              LDR     PC, [PC, #-0x0120]     @ Vector from VicVectAddr
                LDR     PC, FIQ_Addr

Reset_Addr:      .word     Reset_Handler
Undef_Addr:      .word     Undef_Handler
SWI_Addr:        .word     SWI_Handler
PAbt_Addr:       .word     PAbt_Handler
DAbt_Addr:       .word     DAbt_Handler
                 .word     0                    @ Reserved Address 
IRQ_Addr:        .word     IRQ_Handler
FIQ_Addr:        .word     FIQ_Handler

                 .extern  task1_stack   
task0_stack_end: .word     task1_stack + 1024

Undef_Handler:   B       Undef_Handler
SWI_Handler:     B       swi_handler  
PAbt_Handler:    B       PAbt_Handler
DAbt_Handler:    B       DAbt_Handler
IRQ_Handler:     B       IRQ_Handler
FIQ_Handler:     B       FIQ_Handler


				.extern  switchtask
swi_handler: 
				STMFD	sp!, {r0-r12, r14}			@ Save context
				LDR		r10, [r14, #-4]				@ Get svc instruction
				bic		r10, r10, #0xFF000000		@ Get swi num
				mov 	r1,  r13					
				mrs		r2, spsr					@ save spsr, in case call nested swi
				STMFD   sp!, {r2}
				
				BL		switchtask
				
				LDMFD	r13!, {r2}
				MSR		spsr_cxsf, r2
				LDMFD	r13!, {r0-r12,pc}^
                
                .global Reset_handler
Reset_Handler:

@  Call low-level init C-function
@  Setup Stack for each mode
                LDR     R0, =Stack_Top

@  Enter Undefined Instruction Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_UND | I_Bit | F_Bit
                MOV     SP, R0
                SUB     R0, R0, #UND_Stack_Size

@  Enter Abort Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_ABT | I_Bit | F_Bit
                MOV     SP, R0
                SUB     R0, R0, #ABT_Stack_Size

@  Enter FIQ Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_FIQ | I_Bit | F_Bit
                MOV     SP, R0
                SUB     R0, R0, #FIQ_Stack_Size

@  Enter IRQ Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_IRQ | I_Bit | F_Bit
                MOV     SP, R0
                SUB     R0, R0, #IRQ_Stack_Size

@  Enter Supervisor Mode and set its Stack Pointer
                MSR     CPSR_c, #Mode_SVC | I_Bit | F_Bit
                MOV     SP, R0
                SUB     R0, R0, #SVC_Stack_Size

@  Enter User Mode and set its Stack Pointer
@  mt:          MSR     CPSR_c, #Mode_USR
                MSR     CPSR_c, #Mode_USR | I_Bit
                MOV     SP, R0
                SUB     SL, SP, #USR_Stack_Size
@  mt: Start application in USR-mode with IRQ-exceptions disabled.
@  They can be enabled at runtime thru IntEnable in swi.h/swi_handler.S.

@  Relocate .data section (Copy from ROM to RAM)
                LDR     R1, =_etext 
                LDR     R2, =_data 
                LDR     R3, =_edata 
                CMP     R2, R3
                BEQ     DataIsEmpty
LoopRel:        CMP     R2, R3 
                LDRLO   R0, [R1], #4 
                STRLO   R0, [R2], #4 
                BLO     LoopRel 
DataIsEmpty:
 
@  Clear .bss section (Zero init)
                MOV     R0, #0 
                LDR     R1, =__bss_start__ 
                LDR     R2, =__bss_end__ 
                CMP     R1,R2
                BEQ     BSSIsEmpty
LoopZI:         CMP     R1, R2 
                STRLO   R0, [R1], #4 
                BLO     LoopZI 
BSSIsEmpty:


@  Enter the C code
                .extern main
                LDR R0, =main
                BX      R0

				.global switchto
switchto:		
				@ save context
				STMIA	R0!, {R0-R13, R14}  		@ save r0~r14
				STMIA 	R0!, {R14} 					@ save r15
				MRS  	R2,  CPSR       			@ Get cpsr
				STMIA 	R0!, {R2} 					@ save cpsr
				
				
				@ restore context
				LDR		R2,  [R1, #64]				@ Get cpsr
				MSR  	SPSR_cxsf, R2				@ load to spsr
				LDMIA	R1,  {R0-R13, R14, R15}^	@ restore r0~r15, cpsr
                
                .end

